// "168_PWM.c" from Sparkfun "i-Racer" (http://www.sparkfun.com/products/11162)

/*
    referring to the schematic, "motor1" is the rear (drive) motor,     which is on OC1
                                "motor2" is the front (steering) motor, which is on OC0

    now in this software, "motorL" is OC1 => drive motor
                          "motorR" is OC0 => steering motor
*/

#include "168_PWM.h"

void delay_pwm(uchar time)
{
    uchar i,k,j;

    for(k=0;k<time;k++)
    {
        for(j=0;j<8;j++)
        {
            for(i=0;i<248;i++);
        }
    }
}

void PWM_RST(void)
{
	// NOTE: This routine should disable interrupts at the start and enable them at the end.
	//       Need to add this feature.


    DDRB|=(1<<BIT1); // enable OC1A output
    DDRB|=(1<<BIT2); // enable OC1B output
    DDRD|=(1<<BIT5); // enable OC0B output
    DDRD|=(1<<BIT6); // enable OC0A output

    // Timer0 is used for steering
    // Timer1 is used for driving


    // COMnA1:0: = 3, set OCnA high on compare match
    // COMnB1:0: = 3, set OCnB high on compare match
    //WGMn1:0 = 1, in all states
    TCCR0A=0xf1;
	//WGMn3:2 = 0 in all states
    // resultant timer mode(WGM3:0): 1, "PWM, phase correct"
    TCCR0B=0x00;// CSn2:0 = 0, timer/counter stopped

    TCNT0=0X0000;
    OCR0A=0X00EF;
    OCR0B=0X00EF;


    TIFR0 =0xff; // clear all timer interrupts
    TIMSK0=0X00; // disable all timer interrupts

    TCCR1A=0xf1;
    TCCR1B=0x00;

    TCNT1=0X0000;
    OCR1A=0X00EF;
    OCR1B=0X00EF;

    TIFR1 =0xff;
    TIMSK1=0X00;
}

void PWM_EN(uchar PWM_ON)
{
	// NOTE: This routine should disable interrupts at the start and enable them at the end.
	//       Need to add this feature.

	//WGMn3:2 = 0 in all states
	//TCNTn is always reset to zero
	//OCRnA/B is always reset to 0xFF

    if(PWM_ON==0)
    {
        TCCR0B=0x00; // CSn2:0 = 0, timer/counter stopped
        TCNT0=0X0000;
        OCR0A=0X00FF;
        OCR0B=0X00FF;


        TCCR1B=0x00;
        TCNT1=0X0000;
        OCR1A=0X00FF;
        OCR1B=0X00FF;

        return;
    }
    else
    {
        TCNT0=0X0000;
        OCR0A=0X00FF;
        OCR0B=0X00FF;
        TCCR0B=0x01;  // CSn2:0 = 1, timer/counter on, no prescale

        TCNT1=0X0000;
        OCR1A=0X00FF;
        OCR1B=0X00FF;
        TCCR1B=0x01;
        return;
    }
}


// dir = 0 --> Left
// dir = 1 --> Right
// Empty = 0x00 --> turn to "dir"
// Empty = 0xFF --> return to center
void motorR_reg(uchar DIR,uchar Empty) // steering motor
{
    if(DIR==0)
    {
        OCR0B=0Xff;
        delay_pwm(2);  // not sure why there is a difference between steering and driving
        OCR0A=Empty;   // "Empty" is actually the comparison value, it's a terrible variable name.
    }
    else
    {
        OCR0A=0Xff;
        delay_pwm(2);
        OCR0B=Empty;
    }
}

// dir = 0 --> forward
// dir = 1 --> backward
// Empty = speed --> (0x00-0xFF) = max speed - no drive
void motorL_reg(uchar DIR,uchar Empty) // drive motor
{
    if(DIR==0)
    {
        OCR1B=0Xff;
        delay_pwm(5); // not sure why there is a difference between steering and driving
        OCR1A=Empty;
    }
    else
    {
        OCR1A=0Xff;
        delay_pwm(5);
        OCR1B=Empty;
    }
}


